#include <pcl/filters/passthrough.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_ros/point_cloud.h>

typedef pcl::PointCloud<pcl::PointXYZ> PointCloud;


class Subandpub {
public:
  Subandpub() {
    pub_ = n_.advertise<PointCloud>("/published_topic", 1);
    sub_ = n_.subscribe("/velodyne_points", 10 , &Subandpub::callback,this);
  }
  void callback(const sensor_msgs::PointCloud2ConstPtr& input) {
    // 原点云获取后进行滤波
    pcl::PCLPointCloud2 pcl_pc2;
    // 把获取到的pointcloud2::constptr转换为pointxyz
    pcl_conversions::toPCL(*input, pcl_pc2);
    pcl::PointCloud<pcl::PointXYZ>::Ptr tmp_cloud (new pcl::PointCloud<pcl::PointXYZ>);
    pcl::fromPCLPointCloud2(pcl_pc2, *tmp_cloud);

    pcl::PassThrough<pcl::PointXYZ> pass; // 创建滤波器对象
    pass.setInputCloud(tmp_cloud);       // 设置输入点云
    pass.setFilterFieldName("y"); // 滤波字段名被设置为Z轴方向
    pass.setFilterLimits(-8, 16); // 可接受的范围为（0.0，1.0）
    pass.setFilterLimitsNegative(false); // 设置保留范围内还是过滤掉范围内
    pass.filter(*tmp_cloud); // 执行滤波，保存过滤结果在cloud_filtered
    sensor_msgs::PointCloud2 output_cloud;
    pcl::toROSMsg(*tmp_cloud,output_cloud);

    pub_.publish(tmp_cloud);
  }

private:
  ros::NodeHandle n_;
  ros::Publisher pub_;
  ros::Subscriber sub_;
};

int main(int argc, char **argv) {
  ros::init(argc, argv, "pointcloud_f");
  ros::NodeHandle nh;

  Subandpub sub_pub;

  ros::spin();

  return 0;
}
